/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/helpSrc/mark_check.cpp
 * @Description  : MarkCheck类用于管理和检测AGV导航中的二维码路径。
 *                 该类提供了初始化、更新参数、添加新路径、启用/禁用、清除缓冲区、检查ID等功能。
 *                 它通过周期性任务检查二维码ID的有效性并处理丢失的ID。
 *                 类中包含多个私有成员变量用于存储状态和缓冲数据，并使用互斥锁确保线程安全。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:51:47
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include "mark_check.h"
#include "public_kernel.h"
#include "tf2/utils.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

extern WorkStatusRecord VhcStatus;

/**
 * @brief MarkCheck 类的构造函数
 *
 * 初始化 MarkCheck 对象，设置默认参数。
 *
 * @details 构造函数会初始化以下成员变量：
 * - initialized_：标记是否已初始化，默认为 false
 * - enabled_：标记是否启用，默认为 false
 * - max_miss_distance_：允许的最大缺失距离，默认为 0.3f
 * - max_miss_code_count_：允许的最大缺失代码数，默认为 0
 *
 * 构造函数会调用 Reset 方法重置 MarkCheck 对象的状态。
 */
MarkCheck::MarkCheck() 
    : initialized_(false), 
      enabled_(false), 
      max_miss_distance_(0.3f), 
      max_miss_code_count_(0) 
{
    Reset();
}

/**
 * @brief 析构函数
 *
 * 在对象销毁时调用，解锁互斥锁并等待线程结束
 */
MarkCheck::~MarkCheck()
{
    quit_mutex_.unlock();
    thread_.join();
}

/**
 * @brief 更新MarkCheck参数
 *
 * 从参数服务器中读取MarkCheck模块的相关参数，并进行更新。
 */
void MarkCheck::UpdateParam()
{
    max_miss_distance_ = ROSNode()->get_parameter_or("qr_mark_check.miss_distance", 0.3f);
    max_miss_code_count_ = ROSNode()->get_parameter_or("qr_mark_check.miss_code_count", 0);
    DEBUG_OUT("MarkCheck: UpdateParam ok; max_miss_distance: " << max_miss_distance_ 
            << ", max_miss_code_count: " << max_miss_code_count_);
}

void MarkCheck::UpdateProgRunData(USHORT from_node, USHORT to_node, float abs_progress, float prog_percent) 
{
    std::lock_guard<std::mutex> locker(mutex_);
    if (!initialized_ || !enabled_) {
        DEBUG_ERROR_OUT("Error: MarkCheck: UpdateProgRunData failed, initialized: " << initialized_ 
                        << ", enabled: " << enabled_);
        return;
    }
    current_run_data_.Update(from_node, to_node, abs_progress, prog_percent);
}

/**
 * @brief 向MarkCheck中添加新的路径
 *
 * 向MarkCheck中添加新的路径，并进行一系列检查和更新操作。
 *
 * @param path 路径对象指针
 * @param traj 轨迹对象指针
 * @return 如果添加成功，则返回true；否则返回false
 */
bool MarkCheck::AddNewPath(Path* path, Trajectory* traj) 
{
    std::lock_guard<std::mutex> locker(mutex_);
    if (!initialized_ || !enabled_) {
        DEBUG_ERROR_OUT("Error: MarkCheck AddNewLandMark failed, initialized: " << initialized_ 
                        << ", enabled: " << enabled_);
        return false;
    }

    // #1 轨迹对象不存在
    if (!path || !traj) {
        DEBUG_OUT("AddNewLandMark: path or traj is null");
        return false;
    }

    // #2 轨迹类型不匹配
    int traj_type = traj->GetType();
    bool traj_type_valid = (int)TrajType::BEZIER_TRAJ == traj_type || 
                           (int)TrajType::LINE_TRAJ == traj_type || 
                           (int)TrajType::SIDE_TRAJ == traj_type;
    if (!traj_type_valid) {
        DEBUG_OUT("AddNewLandMark: TrajType is not valid: traj_type = " << traj_type);
        return false;
    }

    // 清除之前检测一下是否是最后一个待检测二维码的ID
    if (current_mark_id_.IsNew(500)) {
        if (!check_id_buffer_.empty()) {
            // 最后一个二维码ID
            USHORT last_id = check_id_buffer_.back();
            if (last_id == current_mark_id_.Get()) {
                DEBUG_OUT("AddNewLandMark: last_id == current_mark_id ok; check_id_buffer_.pop_back"
                          << ", check_id_buffer_.size = " << (int)(check_id_buffer_.size()) 
                          << ", last_id = " << last_id);
                check_id_buffer_.pop_back(); // 弹出最后一个待检测ID数据
            }
        } else {
            DEBUG_OUT("AddNewLandMark: check_id_buffer_.size is 0");
        }
    } else {
        DEBUG_ERROR_OUT("AddNewLandMark: current_mark_id_ ");
    }

    // 强制清除一下上一次路段和当前二维码ID的数据
    auto last_mark_id = current_mark_id_.Get();
    current_mark_id_.Update(65535);
    current_run_data_.Clear();
    DEBUG_WARN_OUT("AddNewPath: force clear current_run_data, set current landmark: " 
                   << current_mark_id_.Get() << ", last_mark_id = " << last_mark_id);

    // 下一个节点是否是二维码点(id != -1)
    int next_qr_id = -1;
    try {
        Node end_node = path->GetEndNode();
        next_qr_id = end_node.GetQrId();
        if (next_qr_id == -1) {
            DEBUG_OUT("Next node qrId is not qrCode id");
            return false;
        }
    } catch (...) {
        DEBUG_ERROR_OUT("Error: find next end node crash");
        return false;
    }

    // #4 启终点相同
    USHORT start_id = path->start_node_;
    USHORT end_id = path->end_node_;
    bool same = start_id == end_id;
    if (same) {
        DEBUG_OUT("AddNewLandMark: node id is the same; start_id = " << start_id 
                    << ", end_id = " << end_id);
        return false;
    }

    if (IsIdValid(next_qr_id)) {
        check_id_buffer_.push_back(next_qr_id);
        int size = check_id_buffer_.size();
        DEBUG_OUT("AddNewLandMark ok; start_id = " << start_id 
                  << ", end_id = " << end_id 
                  << ", next_qr_id = " << next_qr_id 
                  << ", check_id_buffer_.size = " << size);
        DEBUG_OUT("markCheck size is: size = " << size);
        for (int i = 0; i < size; i++) {
            DEBUG_OUT("[" << i << "] = " << (int)(check_id_buffer_[i]));
        }
    } else {
        DEBUG_ERROR_OUT("Error: AddNewLandMark end_id: " << end_id 
                        << " not find qrId: " << next_qr_id);
    }
    return true;
}

/**
 * @brief 设置MarkCheck的启用状态
 *
 * 根据传入的enable参数，设置MarkCheck的启用状态，并根据启用状态执行相应的操作。
 *
 * @param enable 是否启用MarkCheck，true表示启用，false表示禁用
 */
void MarkCheck::SetEnable(bool enable) 
{
    std::lock_guard<std::mutex> locker(mutex_);
    enabled_ = enable;
    if (!enabled_) {
        DEBUG_OUT("MarkCheck set disable; clear data; check_id_buffer_.size = " << check_id_buffer_.size() 
                        << ", missed_id_buffer_.size = " << missed_id_buffer_.size());
        Reset();
    } else {
        UpdateParam();
    }
    DEBUG_OUT("MarkCheck SetEnable: enable = " << enable);
}

/**
 * @brief 清除检查缓冲区
 *
 * 该函数用于清除MarkCheck类的检查缓冲区。首先，它会锁定互斥锁以确保线程安全，
 * 然后记录检查ID缓冲区，重置状态，并输出调试信息表示操作成功。
 */
void MarkCheck::ClearCheckBuffer() 
{
    std::lock_guard<std::mutex> locker(mutex_);
    LogCheckIdBuffer();
    Reset();
    DEBUG_OUT("ClearCheckBuffer ok");
}

/**
 * @brief 记录检查ID缓冲区的内容
 *
 * 将检查ID缓冲区的内容输出到调试日志中。
 */
void MarkCheck::LogCheckIdBuffer() 
{
    int size = check_id_buffer_.size();
    DEBUG_OUT("ClearCheckBuffer check_id_buffer_; size = " << size);
    for (int i = 0; i < size; i++) {
        DEBUG_OUT("[" << i << "] = " << (int)(check_id_buffer_[i]));
    }
}

/**
 * @brief 检查一个ID
 *
 * 该函数用于检查待检测的ID，并根据具体情况进行相应处理。
 */
void MarkCheck::CheckOneId() 
{
    int size = check_id_buffer_.size();
    if (size != 1) {
        return;
    }

    USHORT front_id = check_id_buffer_.front();
    // 如果等于起点id看是否超过距离阈值
    if (IsIdSame(front_id, GetNodeQrId(current_run_data_.from_node))) {
        bool distance_over_limit = current_run_data_.abs_progress >= max_miss_distance_;
        if (distance_over_limit) {
            missed_id_buffer_.push_back(front_id);
            check_id_buffer_.pop_back(); // 弹出最后一个待检测ID数据
            DEBUG_ERROR_OUT("size#11: MissId: the same as fromNode distance_over_limit: front_id = " << front_id);
            return;
        }
        if (current_mark_id_.IsNew(500) && front_id == current_mark_id_.Get()) {
            check_id_buffer_.pop_back(); // 弹出最后一个待检测ID数据
            if (!Trigger()) {
                missed_id_buffer_.clear();
                DEBUG_OUT("size#12: find new id ok; try clear missed id buffer");
            }
            DEBUG_OUT("find qrId ok the same as fromId; front_id = " << front_id);
        }
        // 如果检测ID是终点，全程检测
    } else if (IsIdSame(front_id, GetNodeQrId(current_run_data_.to_node))) { 
        if (current_mark_id_.IsNew(500) && front_id == current_mark_id_.Get()) {
            check_id_buffer_.pop_back(); // 弹出最后一个待检测ID数据
            if (!Trigger()) {
                missed_id_buffer_.clear();
                DEBUG_OUT("size#13: find new id ok; try clear missed id buffer");
            }
            DEBUG_OUT("size#14: find qrId ok; the same as toId; front_id = " << front_id);
        }
    } else {
        DEBUG_ERROR_OUT("#15: error: logic is error; front_id: " << front_id 
                        << ", from_node: " << current_run_data_.from_node 
                        << ", to_node: " << current_run_data_.to_node);
        LogCheckIdBuffer();
        Reset();
    }
}

/**
 * @brief 检查两个ID是否符合预期条件
 *
 * 此函数检查存储在check_id_buffer_中的两个ID是否符合当前运行数据的预期条件。
 *
 * 如果check_id_buffer_中的ID数量不为2，则直接返回。
 * 否则，它会检查第一个ID是否与from_node的QR码ID相同，第二个ID是否与to_node的QR码ID相同。
 * 如果找到符合条件的ID，则会根据具体情况执行不同的操作，如标记丢失的ID、清除检测标志等。
 * 如果ID不匹配，则会记录错误信息并重置状态。
 */
void MarkCheck::CheckTwoId() 
{
    int size = check_id_buffer_.size();
    if (size != 2) {
        return;
    }

    USHORT front_id = check_id_buffer_.at(0);
    USHORT next_id = check_id_buffer_.at(1);
    if (IsIdSame(front_id, GetNodeQrId(current_run_data_.from_node)) && 
        IsIdSame(next_id, GetNodeQrId(current_run_data_.to_node))) {
        // 如果已经找到了下一个，上一个还未发现，则上一个id丢失
        if (current_mark_id_.IsNew(500) && next_id == current_mark_id_.Get()) {
            DEBUG_OUT("size#21; find next id but last id not find; front_id: " << front_id << ", miss");
            missed_id_buffer_.push_back(front_id);
            check_id_buffer_.erase(check_id_buffer_.begin());
            if (!Trigger()) {
                missed_id_buffer_.clear();
                DEBUG_OUT("size#2 find new id ok; try clear missed id buffer");
            }
            DEBUG_OUT("size#22: find qrId ok; current_mark_id = " << current_mark_id_.Get());
            return;
        }

        // 起点ID超过距离阈值，报丢失
        bool distance_over_limit = current_run_data_.abs_progress >= max_miss_distance_;
        if (distance_over_limit) {
            missed_id_buffer_.push_back(front_id);
            check_id_buffer_.erase(check_id_buffer_.begin()); // 删除第一个
            DEBUG_ERROR_OUT("size#23: MissId: the same as fromNode distance_over_limit: front_id = " << front_id);
            return;
        }

        // 找到起点ID，清除检测标志
        if (current_mark_id_.IsNew(500) && front_id == current_mark_id_.Get()) {
            check_id_buffer_.erase(check_id_buffer_.begin()); // 删除第一个
            if (!Trigger()) {
                missed_id_buffer_.clear();
                DEBUG_OUT("size#24: find new id ok; try clear missed id buffer");
            }
            DEBUG_OUT("size#25: find qrId ok; the same as fromId; front_id = " << front_id);
            return;
        }
    } else {
        DEBUG_ERROR_OUT("#26 error: logic is error; front_id: " << front_id 
                        << ", next_id: " << next_id 
                        << ", from_node: " << current_run_data_.from_node 
                        << ", to_node: " << current_run_data_.to_node);
        LogCheckIdBuffer();
        Reset();
    }
}

/**
 * @brief 检查其他尺寸ID
 *
 * 该函数用于检查其他尺寸ID的逻辑是否正确。
 *
 * @note 如果检查ID缓冲区的大小小于等于2，则不进行任何操作。
 *       如果大小大于2，则记录错误信息并重置。
 */
void MarkCheck::CheckOtherSizeId() 
{
    int size = check_id_buffer_.size();
    if (size <= 2) {
        return;
    }
    DEBUG_ERROR_OUT("#31 error: logic is error"
                    << ", from_node: " << current_run_data_.from_node 
                    << ", to_node: " << current_run_data_.to_node);
    LogCheckIdBuffer();
    Reset();
}

/**
 * @brief CycleRoutine 函数
 *
 * 此函数为 MarkCheck 类的成员函数，用于执行周期性任务。
 *
 * 在执行周期性任务之前，首先加锁，确保线程安全。
 * 如果未初始化或未启用，则直接返回。
 * 如果当前节点无效，则直接返回。
 * 如果检查ID缓冲区为空，则直接返回。
 *
 * 根据检查ID缓冲区的大小执行不同的检查逻辑：
 * - 如果缓冲区大小为1，则调用 CheckOneId 函数进行检查，并输出调试信息。
 * - 如果缓冲区大小为2，则调用 CheckTwoId 函数进行检查，并输出调试信息。
 * - 如果缓冲区大小不为1或2，则调用 CheckOtherSizeId 函数进行检查，并输出调试信息。
 *
 * @return 无返回值
 */
void MarkCheck::CycleRoutine() 
{
    std::lock_guard<std::mutex> locker(mutex_);
    if (!initialized_ || !enabled_) {
        return;
    }
    if (!IsNodeValid()) {
        return;
    }
    if (check_id_buffer_.empty()) {
        return;
    }

    int size = check_id_buffer_.size();
    if (size == 1) {
        // #1 缓冲区存在一个ID
        CheckOneId();
        DEBUG_OUT("mark check: CheckOneId:"
                  << ", current_from_node: " << current_run_data_.from_node
                  << ", current_to_node: " << current_run_data_.to_node);
    } else if (size == 2) {
        // #2 缓冲区存在两个检测ID
        CheckTwoId();
        DEBUG_OUT("mark check: CheckTwoId:"
                  << ", current_from_node: " << current_run_data_.from_node
                  << ", current_to_node: " << current_run_data_.to_node);
    } else {
        // #3 错误逻辑
        CheckOtherSizeId();
        DEBUG_OUT("mark check: CheckOtherSizeId:"
                  << ", current_from_node: " << current_run_data_.from_node
                  << ", current_to_node: " << current_run_data_.to_node);
    }
}

/**
 * @brief 线程函数
 *
 * 该函数是 MarkCheck 类的一个成员函数，用于实现线程的循环执行逻辑。
 *
 * 在函数内部，使用 try_lock_for 方法尝试在指定的时间内锁定 quit_mutex_ 互斥锁。
 * 如果在指定时间内成功锁定互斥锁，则退出循环；否则，调用 CycleRoutine 函数执行循环逻辑。
 */
void MarkCheck::ThreadFunction() 
{
    while (!quit_mutex_.try_lock_for(std::chrono::milliseconds(10))) {
        CycleRoutine();
    }
}

/**
 * @brief 回调函数，用于处理获取到的地标数据
 *
 * 当接收到地标（二维码）数据时，该函数将被调用。
 * 函数内部会先加锁，防止地标ID被意外清除，然后提取地标数据中的位置信息，
 * 更新当前地标ID，并尝试打印地标ID和地标位置信息。
 *
 * @param mark_data 包含地标位置和姿态的PoseStamped消息
 */
void MarkCheck::GetLandMarkCallback(const geometry_msgs::msg::PoseStamped& mark_data) 
{
    std::lock_guard<std::mutex> locker(mutex_);     // 增加锁 防止对当前获取的二维码id意外清除
    geometry_msgs::msg::Pose real_pose = mark_data.pose; // 二维码计算出来相对车体中心的位姿数据（暂时不需要）
    current_mark_id_.Update(real_pose.position.z);  // 二维码读取出来的ID
    try {
        DEBUG_OUT("landmark: " << current_mark_id_.Get() << ", qrPos: x = " << real_pose.position.x
                               << ", y = " << real_pose.position.y
                               << ", (degree) yaw = " << TO_DEGREE(Angle::NormAngle2(tf2::getYaw(real_pose.orientation))));
    } catch (...) {
        DEBUG_ERROR_OUT("error: landmark yaw is illegal");
    }
}

/**
 * @brief 初始化MarkCheck类
 *
 * 该函数用于初始化MarkCheck类，包括订阅主题、启动线程和更新参数等。
 *
 * @return 如果初始化成功，返回true；否则返回false。
 */
bool MarkCheck::Init() 
{
    if (initialized_) {
        DEBUG_OUT("MarkCheck init finished before");
        return true;
    }

    landmark_subscriber_ = ROSNode()->create_subscription<geometry_msgs::msg::PoseStamped>(
        "/landmark", 28, 
        std::bind(&MarkCheck::GetLandMarkCallback, this, std::placeholders::_1)
    ); // 订阅 landmark
    
    quit_mutex_.lock();
    thread_ = std::thread(std::bind(&MarkCheck::ThreadFunction, this));
    UpdateParam();
    DEBUG_OUT("MarkCheck init finished first");
    initialized_ = true;
    return true;
}

/**
 * @brief 重置 MarkCheck 类的状态
 *
 * 该函数将 MarkCheck 类的状态重置为初始状态。
 *
 * @details
 * - 清空当前标记 ID（current_mark_id_）并将其更新为 65535。
 * - 清空当前运行数据（current_run_data_）。
 * - 清空检查 ID 缓冲区（check_id_buffer_）。
 * - 清空遗漏 ID 缓冲区（missed_id_buffer_）。
 *
 * 在执行过程中，会通过 DEBUG_OUT 输出调试信息，包括检查 ID 缓冲区和遗漏 ID 缓冲区的大小。
 */
void MarkCheck::Reset() 
{
    DEBUG_OUT("MarkCheck; clear data; check_id_buffer_.size = " << check_id_buffer_.size()
              << ", missed_id_buffer_.size = " << missed_id_buffer_.size());
    current_mark_id_.Update(65535);
    current_run_data_.Clear();
    check_id_buffer_.clear();
    missed_id_buffer_.clear();
}

/**
 * @brief 检查节点是否有效
 *
 * 检查当前运行数据中的起始节点和终止节点是否满足以下条件：
 * 1. 起始节点和终止节点不相同。
 * 2. 起始节点和终止节点的ID都有效。
 *
 * @return 如果节点有效，则返回true；否则返回false。
 */
bool MarkCheck::IsNodeValid() 
{
    bool same = current_run_data_.from_node == current_run_data_.to_node;
    bool valid = !same && IsIdValid(current_run_data_.from_node) && IsIdValid(current_run_data_.to_node);
    return valid;
}

/**
 * @brief 获取节点的二维码ID
 *
 * 根据给定的节点ID获取对应的二维码ID。
 *
 * @param node_id 节点ID
 * @return 返回对应节点的二维码ID
 */
USHORT MarkCheck::GetNodeQrId(USHORT node_id) 
{
    return VhcStatus.GetQrCodeId(node_id);
}

/**
 * @brief 触发检查
 *
 * 检查是否需要触发某些操作，如发送报警等。
 *
 * @return 如果满足触发条件则返回 true，否则返回 false
 */
bool MarkCheck::Trigger() 
{
    if (!enabled_) {
        return false;
    }
    return missed_id_buffer_.size() > static_cast<std::size_t>(max_miss_code_count_);
}

/**
 * @brief 触发检查标记
 *
 * 如果检查标记已启用，则将错过ID缓冲区中的所有ID拼接到missed_id字符串中，并以分号分隔。
 * 如果错过ID的数量超过最大允许值，则返回true，否则返回false。
 *
 * @param missed_id 用于存储拼接后的错过ID的字符串
 * @return 如果错过ID的数量超过最大允许值，则返回true；否则返回false
 */
bool MarkCheck::Trigger(std::string& missed_id) 
{
    if (!enabled_) {
        return false;
    }
    for (std::size_t i = 0; i < missed_id_buffer_.size(); i++) {
        missed_id += std::to_string(missed_id_buffer_[i]) + ";";
    }
    return missed_id_buffer_.size() > static_cast<std::size_t>(max_miss_code_count_);
}

/**
 * @brief 检查两个USHORT类型的ID是否相同
 *
 * 该函数用于比较两个USHORT类型的ID是否相同。
 *
 * @param id1 第一个要比较的ID
 * @param id2 第二个要比较的ID
 *
 * @return 如果id1等于id2，则返回true；否则返回false
 */
bool MarkCheck::IsIdSame(USHORT id1, USHORT id2) 
{
    return id1 == id2;
}

/**
 * @brief 检查ID是否有效
 *
 * 判断给定的USHORT类型的ID是否有效。
 *
 * @param id 要检查的ID
 * @return 如果ID不等于65535，则返回true，表示ID有效；否则返回false，表示ID无效
 */
bool MarkCheck::IsIdValid(USHORT id) 
{
    return id != 65535;
}